#!/usr/bin/python2.7
# -*- coding: utf-8 -*-

import imp
import time
import socket
import struct
import sys
from datetime import datetime
import os
import argparse

import rospy
import sensor_msgs.point_cloud2 as pc2

import dynamic_reconfigure.client


from sensor_msgs.msg import PointCloud2, PointField
from std_msgs.msg import Header
from visualization_msgs.msg import Marker,MarkerArray

from math import sin,cos,pi
# from statistics import mode #数据统计库
import numpy as np
from cmath import sqrt
# from zlib import Z_PARTIAL_FLUSH

from publish_utils import publish_azimuthLines,publish_piontcloud_marker,publish_pointclouds
from ars_parse_detection import ArsParseDetection



class ARS548():
    def __init__(self):
        '''
            @ home_path: raw_data save path
            @ online_flag:如果连接了雷达，为 True 否则为 False
            @ record_raw_data_flag: 是否记录原始数据
        '''
        multicast_group = '224.0.2.2'
        # server_address = ('192.168.1.10',42103)
        server_address = ('',42102)
        # Create the socket
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

        # Bind to the server address
        self.sock.bind(server_address)

        # Tell the operating system to add the socket to the multicast group
        # on all interfaces.
        group = socket.inet_aton(multicast_group)
        mreq = struct.pack('4sL', group, socket.INADDR_ANY)
        self.sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
        self.sock.settimeout(1)

    def data_parse(self,data):
        data_length = len(data)
        if(data_length==35336):
            ars_detection = ArsParseDetection.from_bytes(data)

            x_list = []
            y_list = []
            z_list = []
            doppler_v_list=[]
            rcs_list=[]
            temp_valid_data_len = 0
            for id,d in enumerate(ars_detection.body.list_detections):
                if(d.f_range != 0):
                    temp_valid_data_len = id
                    # calculate x,y,z
                    y = sin(d.f_azimuthangle)*d.f_range
                    z = sin(d.f_elevationangle)*d.f_range
                    x = sqrt(d.f_range**2-y**2-z**2)
                  
                    x_list.append(x)
                    y_list.append(y)
                    z_list.append(z)
                    doppler_v_list.append(d.f_rangerate)
                    rcs_list.append(d.s_rcs)


            x = np.array(x_list)
            y = np.array(y_list)
            z = np.array(z_list)
            doppler_v_array = np.array(doppler_v_list)
            rcs_list_array = np.array(rcs_list)



            single_inte = np.ones((temp_valid_data_len+1))

            pointclouds = np.stack((x, y, z, doppler_v_array,rcs_list_array),1)


            return True,pointclouds
        elif(data_length==9401):
            # ars_object = ArsParseObject.from_bytes(data)
            # print('method_id',ars_object.header.method_id)
            # print(ars_object.header.length)
            # print(ars_object.header.protocol_version)

            # print('u_shape_width_edge_std',ars_object.body.objectlist_objects[0].u_shape_width_edge_std)
           
            return False,[]
        elif(data_length==158):
            return False,[]
        elif(data_length==803):       
            return False,[]
        elif(data_length==50):
            return False,[]
        else:
            return False,[]
            # print("other data")

if __name__=="__main__":
    rospy.init_node('ars548_node')
    print("ARS548 NODE START")
    cloudpoints_pub = rospy.Publisher("/ars548/cloudpoints", PointCloud2, queue_size=10)
    azimuthLines_pub = rospy.Publisher('/ars548/azimuthLines', Marker, queue_size = 10) 
    cloudmarkerarray_pub = rospy.Publisher("/ars548/cloudmarkerarray", MarkerArray, queue_size=10)
    ars_node = ARS548()
    rate = rospy.Rate(100)  
    while not rospy.is_shutdown():
        marker_ests = MarkerArray()
        try:
            # print(ars_node.sock.getsockname())
            data, address = ars_node.sock.recvfrom(50000)
            # print(address)
            ret,cloudpoints = ars_node.data_parse(data)
            if(ret):
                publish_azimuthLines(azimuthLines_pub)
                publish_pointclouds(cloudpoints_pub,cloudpoints)
                rate.sleep()
        except BaseException as Argument:
             print("[ARS548 NODE]" + str(Argument))
